﻿/*
 * Created by SharpDevelop.
 * User: Fredrik Haglund
 * Date: 2012-01-18
 * Time: 15:08
 * 
 * To change this template use Tools | Options | Coding | Edit Standard Headers.
 */
using System;

namespace RWXMLEdit.Common
{
	/// <summary>
	/// Description of Class1.
	/// </summary>
	public class RwMatrix
	{
		private double[,] _rotM;
		private double _rotZ, _rotY, _rotX;
		private double _transZ, _transY, _transX;
		
		//Constructor
		public RwMatrix()
		{
			_rotM = new double[4,4];
			//UnityMatrix
			ResetMatrix();
		}
		
		//Destructor
		~RwMatrix()
		{
			_rotM = null;
		}
		//Get and Set Matrix
        public void SetMatrix(double[] inArray)
        {
            int k = 0;
            for (int i = 0; i < 4; i++)
            {
                for (int j = 0; j < 4; j++)
                {
                    this.SetElement(i,j,inArray[k]);
                    k++;
                }
            }

            this.CalculateRotationAngles();
        }
        public double[] GetMatrix()
        {
            double[] outArray=new double[16];

            int k = 0;
            for (int i = 0; i < 4; i++)
            {
                for (int j = 0; j < 4; j++)
                {
                    outArray[k] = this.GetElement(i, j);
                    k++;
                }
            }
            return outArray;
        }
		//Get and Set Methods for the matrix elements indices from 0,0 through 3,3
		public void SetElement(int i,int j,double val)
		{
			_rotM.SetValue(val,i,j);
		    _transX = GetElement(3, 0);
		    _transZ = GetElement(3, 1);
		    _transY = GetElement(3, 2);
		}

		public double GetElement(int i,int j)
		{
			return Convert.ToDouble(_rotM.GetValue(i,j));
		}
		
		//Get and Set Rotation around Z-axle
		public double GetZRotation()
		{
			CalculateRotationAngles();
			return _rotZ;
		}
		public void SetZRotation(double zAngle)
		{
			_rotZ=zAngle;
			CreateRotationMatrix();
		}
		
		//Get and Set Rotation around Y-axle
		public double GetXRotation()
		{
			CalculateRotationAngles();
			return _rotY;
		}
		public void SetXRotation(double yAngle)
		{
			_rotY=yAngle;
			CreateRotationMatrix();
		}
		
		//Get and Set Rotation around X-axle
		public double GetYRotation()
		{
			CalculateRotationAngles();
			return _rotX;
		}
		public void SetYRotation(double xAngle)
		{
			_rotX=xAngle;
			CreateRotationMatrix();
		}
		
		//Get and Set Translation in the direction of the x-axle
		public double GetXTranslation()
		{
			return _transX;
		}
		public void SetXTranslation(double xTranslation)
		{
			_transX=xTranslation;
            SetElement(3, 0, _transX);
			CreateRotationMatrix();
		}
		
		//Get and Set Translation in the direction of the z-axle
		public double GetZTranslation()
		{
			return _transZ;
		}
		public void SetZTranslation(double zTranslation)
		{
			_transZ=zTranslation;
            SetElement(3, 1, _transZ);
			CreateRotationMatrix();
		}
		
		//Get and Set Translation in the direction of the y-axle
		public double GetYTranslation()
		{
			return _transY;
		}
		public void SetYTranslation(double yTranslation)
		{
			_transY=yTranslation;
            SetElement(3, 2, _transY);
			CreateRotationMatrix();
		}
		
		//ResetMatrix creates a Unitymatrix as defaultvalue ie no translation or rotation
		public void ResetMatrix()
		{
			for (int i=0;i<4;i++) {
				for(int j=0;j<4;j++)
				{
					_rotM.SetValue(0,i,j);
				}
			}
			_rotM.SetValue(1,0,0);
			_rotM.SetValue(1,1,1);
			_rotM.SetValue(1,2,2);
			_rotM.SetValue(1,3,3);
		}
		
		//Create the rotation matrix from given angles and positions
		public void CreateRotationMatrix()
		{
			//convert Angles to Radians and switch from DirectX mode to Euler
			double xPitch = _rotY * Math.PI / 180;
			double yYaw =_rotZ * Math.PI / 180;

			double zRoll =_rotX * Math.PI / 180;

			//Rotation Values
			SetElement(0,0,Math.Round(Math.Cos(zRoll)*Math.Cos(yYaw)+Math.Sin(zRoll)*Math.Sin(xPitch)*Math.Sin(yYaw),6));
			SetElement(0,1,Math.Round(Math.Sin(zRoll)*Math.Cos(xPitch),6));
			SetElement(0,2,Math.Round(-Math.Cos(zRoll)*Math.Sin(yYaw)+Math.Sin(zRoll)*Math.Sin(xPitch)*Math.Cos(yYaw),6));
			
			SetElement(1,0,Math.Round(-Math.Sin(zRoll)*Math.Cos(yYaw)+Math.Cos(zRoll)*Math.Sin(xPitch)*Math.Sin(yYaw),6));
			SetElement(1,1,Math.Round(Math.Cos(zRoll)*Math.Cos(xPitch),6));
			SetElement(1,2,Math.Round(Math.Sin(zRoll)*Math.Sin(yYaw)+Math.Cos(zRoll)*Math.Sin(xPitch)*Math.Cos(yYaw),6));
			
            SetElement(2,0,Math.Round(Math.Cos(xPitch)*Math.Sin(yYaw),6));
			SetElement(2,1,Math.Round(-Math.Sin(xPitch),6));
			SetElement(2,2,Math.Round(Math.Cos(xPitch)*Math.Cos(yYaw),6));
			
            SetElement(3,0,_transX);
			SetElement(3,1,_transZ);
			SetElement(3,2,_transY);
		}
		
		//Calculate the rotation angles from the given rotation matrix
		public void CalculateRotationAngles()
		{
			double xPitch,yYaw,zRoll;
			xPitch=Math.Asin(-GetElement(2,1));
			double th=0.00001;
			double test=Math.Cos(xPitch);
			if(test>th){
				zRoll=Math.Atan2(this.GetElement(0,1),this.GetElement(1,1));
				yYaw=Math.Atan2(this.GetElement(2,0),this.GetElement(2,2));
			}
			else{
				zRoll=Math.Atan2(-this.GetElement(1,0),this.GetElement(0,0));
				yYaw=0.0;
			}
			//convert from radians to degrees
			zRoll=zRoll*180/Math.PI;
			yYaw=yYaw*180/Math.PI;
			xPitch=xPitch*180/Math.PI;
			//set angle values in rounded off mode and in normal Euler Space not in DirectX space
			_rotZ=Math.Round(yYaw,1);
			_rotX=Math.Round(zRoll,1);
			_rotY=Math.Round(xPitch,1);
		}
	}
}
